#include "pr2_fk.h"

using namespace Eigen;
using namespace ophys;

static inline double IKsin(double f) { return sin(f); }
static inline double IKcos(double f) { return cos(f); }
#define IkReal double
// from ikfast
static inline Vector3d PR2_ComputeFk_RightArm(const Vector7d &j) {
IkReal x0,x1,x2,x3,x4,x5,x6,x7,x8,x9,x10,x11,x12,x13,x14,x15,x16,x17,x18,x19,x20,x21,x22,x23,x24,x25,x26,x27,x28,x29,x30,x31,x32,x33,x34,x35,x36,x37,x38,x39,x40,x41,x42,x43,x44,x45,x46,x47,x48,x49,x50,x51,x52,x53,x54,x55,x56,x57,x58,x59,x60,x61,x62;
x0=IKcos(j[0]);
x1=IKsin(j[0]);
x2=IKsin(j[2]);
x3=IKcos(j[2]);
x4=IKsin(j[1]);
x5=IKsin(j[4]);
x6=IKcos(j[4]);
x7=IKcos(j[3]);
x8=IKcos(j[1]);
x9=IKsin(j[3]);
x10=IKcos(j[6]);
x11=IKsin(j[6]);
x12=IKsin(j[5]);
x13=IKcos(j[5]);
x14=((IkReal(1.00000000000000))*(x12));
x15=((IkReal(0.180000000000000))*(x7));
x16=((IkReal(0.180000000000000))*(x5));
x17=((IkReal(0.321000000000000))*(x3));
x18=((IkReal(1.00000000000000))*(x1));
x19=((IkReal(0.180000000000000))*(x9));
x20=((IkReal(1.00000000000000))*(x13));
x21=((IkReal(1.00000000000000))*(x9));
x22=((IkReal(0.180000000000000))*(x6));
x23=((IkReal(1.00000000000000))*(x3));
x24=((x8)*(x9));
x25=((x0)*(x8));
x26=((x0)*(x3));
x27=((x0)*(x2));
x28=((x4)*(x7));
x29=((IkReal(-1.00000000000000))*(x7));
x30=((x1)*(x4));
x31=((x1)*(x8));
x32=((x2)*(x8));
x33=((x31)*(x7));
x34=((x21)*(x25));
x35=((x18)*(x24));
x36=((x23)*(x7)*(x8));
x37=((((x27)*(x4)))+(((IkReal(-1.00000000000000))*(x18)*(x3))));
x38=((x27)+(((IkReal(-1.00000000000000))*(x18)*(x3)*(x4))));
x39=((((x2)*(x30)))+(x26));
x40=((((IkReal(-1.00000000000000))*(x36)))+(((x4)*(x9))));
x41=((((IkReal(-1.00000000000000))*(x18)*(x2)))+(((IkReal(-1.00000000000000))*(x0)*(x23)*(x4))));
x42=((x37)*(x5));
x43=((x38)*(x7));
x44=((x39)*(x5));
x45=((((x21)*(x3)*(x8)))+(((IkReal(1.00000000000000))*(x28))));
x46=((x38)*(x9));
x47=((x41)*(x7));
x48=((x41)*(x9));
x49=((((x32)*(x5)))+(((x40)*(x6))));
x50=((((IkReal(-1.00000000000000))*(x1)*(x24)))+(x43));
x51=((((x32)*(x6)))+(((x5)*(((((IkReal(-1.00000000000000))*(x21)*(x4)))+(x36))))));
x52=((((IkReal(-1.00000000000000))*(x18)*(x7)*(x8)))+(((IkReal(-1.00000000000000))*(x21)*(x38))));
x53=((x13)*(x49));
x54=((((IkReal(-1.00000000000000))*(x0)*(x24)))+(x47));
x55=((x50)*(x6));
x56=((((IkReal(-1.00000000000000))*(x48)))+(((x25)*(x29))));
x57=((x54)*(x6));
x58=((x44)+(x55));
x59=((((x6)*(((((IkReal(-1.00000000000000))*(x34)))+(x47)))))+(x42));
x60=((((x5)*(((((x29)*(x38)))+(x35)))))+(((x39)*(x6))));
x61=((((x5)*(((((x29)*(x41)))+(x34)))))+(((x37)*(x6))));
x62=((x13)*(x59));

Vector3d eetrans;
// eerot[0]=((((x11)*(x61)))+(((x10)*(((((x12)*(x56)))+(x62))))));
// eerot[1]=((((x11)*(((((IkReal(-1.00000000000000))*(x20)*(x59)))+(((IkReal(-1.00000000000000))*(x14)*(x56)))))))+(((x10)*(x61))));
// eerot[2]=((((x13)*(((x48)+(((x25)*(x7)))))))+(((x12)*(((x42)+(x57))))));
eetrans[0]=((((x9)*(((((IkReal(-1.00000000000000))*(x0)*(x17)*(x4)))+(((IkReal(-0.321000000000000))*(x1)*(x2)))))))+(((IkReal(0.100000000000000))*(x0)))+(((IkReal(0.321000000000000))*(x25)*(x7)))+(((IkReal(0.400000000000000))*(x25)))+(((x12)*(((((x16)*(x37)))+(((x22)*(x54)))))))+(((x13)*(((((x19)*(x41)))+(((x15)*(x25))))))));
// eerot[3]=((((x11)*(x60)))+(((x10)*(((((x13)*(x58)))+(((x12)*(x52))))))));
// eerot[4]=((((x11)*(((((IkReal(-1.00000000000000))*(x20)*(x58)))+(((IkReal(-1.00000000000000))*(x14)*(x52)))))))+(((x10)*(x60))));
// eerot[5]=((((x13)*(((x33)+(x46)))))+(((x12)*(((((x6)*(((((IkReal(-1.00000000000000))*(x35)))+(x43)))))+(x44))))));
eetrans[1]=((IkReal(-0.188000000000000))+(((IkReal(0.100000000000000))*(x1)))+(((x13)*(((((x19)*(x38)))+(((x15)*(x31)))))))+(((IkReal(0.321000000000000))*(x33)))+(((IkReal(0.400000000000000))*(x31)))+(((x9)*(((((IkReal(-1.00000000000000))*(x17)*(x30)))+(((IkReal(0.321000000000000))*(x27)))))))+(((x12)*(((((x16)*(x39)))+(((x22)*(x50))))))));
// eerot[6]=((((x11)*(x51)))+(((x10)*(((x53)+(((x12)*(x45))))))));
// eerot[7]=((((x10)*(x51)))+(((x11)*(((((IkReal(-1.00000000000000))*(x20)*(x49)))+(((IkReal(-1.00000000000000))*(x14)*(x45))))))));
// eerot[8]=((((x12)*(x49)))+(((IkReal(-1.00000000000000))*(x13)*(x45))));
eetrans[2]=((((x13)*(((((IkReal(-1.00000000000000))*(x19)*(x3)*(x8)))+(((IkReal(-1.00000000000000))*(x15)*(x4)))))))+(((x12)*(((((x16)*(x32)))+(((x22)*(x40)))))))+(((IkReal(-1.00000000000000))*(x17)*(x24)))+(((IkReal(-0.400000000000000))*(x4)))+(((IkReal(-0.321000000000000))*(x28))));

return eetrans;
}



PR2FastFK::PR2FastFK(bool) : m_offset(Vector3d::Zero())
{
}

void PR2FastFK::calibrate(RobotManipulator::Ptr manip) {
  // translation only....
  Vector7d vals; vals << -2.2853, -0.523600,  -3.90000,  -2.32130 ,   -10000 ,    -2.17  ,  -10000;
  manip->setDOFValues(toStlVec(vals));
  m_offset = toEigVec(manip->getTransform().getOrigin()) - jointToPos(vals);
}

Vector3d PR2FastFK::jointToPos(const Vector7d &j) {
  return PR2_ComputeFk_RightArm(j) + m_offset;
}
